/**
 * @file load_2d_map.cpp
 * @brief 读取2D地图并发布为OccupancyGrid消息的ROS2节点
 * 先用build_2d_map建立2D地图后, launch文件启动例如:
 * ros2 launch map_manager load_2d_map.launch.py map_file_path:=maps/6.10lab_mapping/2d_lab_map_-1587_-1537.png
 */
#include <map_manager/utils.hpp>
#include <climits>

std::string root_dir = ROOT_DIR;

class Load2DMapNode: public rclcpp::Node {
public:
    explicit Load2DMapNode(): Node("load_2d_map_node") {
        this->declare_parameter<int>("map_publish_times", 2);
        this->declare_parameter<std::string>("header_frame_id", "map");
        this->declare_parameter<std::string>("map_file_path", "");
        this->declare_parameter<double>("resolution", 0.1);
        map_publish_times_ = this->get_parameter("map_publish_times").as_int();
        std::string header_frame_id = this->get_parameter("header_frame_id").as_string();
        std::string map_file_path = this->get_parameter("map_file_path").as_string();
        double resolution = this->get_parameter("resolution").as_double();

        if (map_publish_times_ == -1) {
            map_publish_times_ = INT_MAX;
        }

        if (map_file_path[0] != '/' || map_file_path[0] != '~') {
            map_file_path = root_dir + "/" + map_file_path;
        }

        map_pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>("grid_map", 10);
        
        if (map_file_path.empty()) {
            RCLCPP_ERROR(this->get_logger(), "Parameter 'map_file_path' must be provided!");
            rclcpp::shutdown();
            return;
        }
        
        grid_ = load_grid_from_image(header_frame_id, map_file_path, resolution);
        publish_map();
    }
private:
    void publish_map() {
        RCLCPP_INFO(this->get_logger(), "Publishing occupancy grid map to /grid_map with %d times", map_publish_times_);
        for (int i = 0; i < map_publish_times_ && rclcpp::ok(); ++i) {
            grid_->header.stamp = this->now();
            map_pub_->publish(*grid_);
            rclcpp::sleep_for(std::chrono::seconds(3));
            if (map_publish_times_ != INT_MAX) {
                RCLCPP_INFO(this->get_logger(), "Occupancy grid map published successfully %d/%d times", i + 1, map_publish_times_);
            }
        }
        RCLCPP_INFO(this->get_logger(), "Finished publishing occupancy grid map with %d times", map_publish_times_);
    }

    int map_publish_times_;
    rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
    std::shared_ptr<nav_msgs::msg::OccupancyGrid> grid_;
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Load2DMapNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}